#include "skindetector.h"

using namespace cv;
using namespace mlgesture;

const int SkinDetector::GSD_HUE_LT = 170;
const int SkinDetector::GSD_HUE_UT = 10;
const int SkinDetector::GSD_INTENSITY_LT = 15;
const int SkinDetector::GSD_INTENSITY_UT = 240;

SkinDetector::SkinDetector(int morphing_method, int user_hue_lt, int user_hue_ut, int user_value_lt, int user_value_ut) :
	histogramHueMotion_(user_hue_lt/2, user_hue_ut/2),
	skinHueHistogram_(user_hue_lt/2, user_hue_ut/2)
{
	morphingMethod_ = morphing_method;
	userHueLt_ = user_hue_lt/2;
	userHueUt_ = user_hue_ut/2;
	userValueLt_ = user_value_lt;
	userValueUt_ = user_value_ut;
	skinHueLowerBound_ = user_hue_lt/2;
	skinHueUpperBound_ = user_hue_ut/2;

	histogramMergeFactor_ = 0.05;
	huePercentCovered_ = 0.95;

	adapt_ = true;
};

SkinDetector::~SkinDetector() {};

void SkinDetector::process(const Mat &input_bgr_image, Mat &output_hue_mask) {
	Mat src = input_bgr_image;

	unsigned char h, v;
	bool is_init = false;

	if (imgMotionFrame_.data == NULL) 	{
		is_init = true;
		imgLastValueFrame_ = Mat(src.rows, src.cols, CV_8UC1);
		imgFilteredFrame_ = Mat(src.rows, src.cols, CV_8UC1);

	}

	imgMotionFrame_ = Mat::zeros(src.rows, src.cols, CV_8UC1);
	imgFilteredFrame_ = Mat::zeros(src.rows, src.cols, CV_8UC1);

	cvtColor(src, imgHSVFrame_, CV_BGR2HSV);
	split(imgHSVFrame_, imgHSVChannels_);

	Size size_tmp = src.size();
	for( int i = 0; i < size_tmp.height; i++ ) {
		const unsigned char* ptr_hsv = imgHSVFrame_.ptr<unsigned char>(i);
		unsigned char* ptr_motion = imgMotionFrame_.ptr<unsigned char>(i);
		const unsigned char* ptr_last_value = imgLastValueFrame_.ptr<unsigned char>(i);
		unsigned char* ptr_filtered = imgFilteredFrame_.ptr<unsigned char>(i);
		for( int j = 0; j < size_tmp.width; ++j ) {
			v = ptr_hsv[j*3+2];
			if ((v >= userValueLt_) && (v <= userValueUt_)) {
				h = ptr_hsv[j*3];
				if (((userHueLt_ < userHueUt_) && (h >= userHueLt_) && (h <= userHueUt_)) ||
						((userHueLt_ > userHueUt_) && ((h >= userHueLt_) || (h <= userHueUt_)))) {

					if (((skinHueLowerBound_ < skinHueUpperBound_) && (h >= skinHueLowerBound_) && (h <= skinHueUpperBound_)) ||
							((skinHueLowerBound_ > skinHueUpperBound_) && ((h >= skinHueLowerBound_) || (h <= skinHueUpperBound_))))
						ptr_filtered[j] = h;

					if (abs(ptr_last_value[j] - v) >= 30)
						ptr_motion[j] = h;
				}
			}
		}
	}

	int channels = 0;
	int hbins = 180;
	int hist_size[] = {hbins};
	float hranges[] = { 0, 180 };
	const float* ranges[] = { hranges };

	if (is_init) {
		calcHist(&imgHSVFrame_, 1, &channels, imgFilteredFrame_, skinHueHistogram_.fHistogram, 1, hist_size, ranges, true, false);
	}

	imgLastValueFrame_ = imgHSVChannels_[2].clone();

	if (adapt_) {
		erode(imgMotionFrame_, imgTemp_, Mat());
		dilate(imgTemp_, imgMotionFrame_, Mat());

		calcHist(&imgMotionFrame_, 1, &channels, imgMotionFrame_, histogramHueMotion_.fHistogram, 1, hist_size, ranges, true, false);

		skinHueHistogram_.mergeWith(histogramHueMotion_, histogramMergeFactor_);
		skinHueHistogram_.findCurveThresholds(skinHueLowerBound_, skinHueUpperBound_, 1 - huePercentCovered_);
	}

	switch (morphingMethod_)
	{
		case MORPHING_METHOD_ERODE :
			erode(imgFilteredFrame_, imgTemp_, Mat());
			imgTemp_.copyTo(imgFilteredFrame_);
			break;
		case MORPHING_METHOD_ERODE_ERODE :
			erode(imgFilteredFrame_, imgTemp_, Mat());
			erode(imgTemp_, imgFilteredFrame_, Mat());
			break;
		case MORPHING_METHOD_ERODE_DILATE :
			erode(imgFilteredFrame_, imgTemp_, Mat());
			dilate(imgTemp_, imgFilteredFrame_, Mat());
			break;
		case MORPHING_METHOD_DILATE :
			dilate(imgFilteredFrame_, imgTemp_, Mat());
			imgTemp_.copyTo(imgFilteredFrame_);
			break;
		case MORPHING_METHOD_DILATE_DILATE :
			dilate(imgFilteredFrame_, imgTemp_, Mat());
			dilate(imgTemp_, imgFilteredFrame_, Mat());
			break;
		case MORPHING_METHOD_DILATE_ERODE :
			dilate(imgFilteredFrame_, imgTemp_, Mat());
			erode(imgTemp_, imgFilteredFrame_, Mat());
			break;
	}

	output_hue_mask = imgFilteredFrame_;
};


SkinDetector::Histogram::Histogram(const int user_hue_lt, const int user_hue_ut) :
	userHueLt_(user_hue_lt),
	userHueUt_(user_hue_ut)
{
	if (userHueUt_ > userHueLt_) {
		histogramSize_ = userHueUt_ - userHueLt_ + 1;
	}
	else {
		histogramSize_ = 179 - userHueLt_ + userHueUt_ + 2;
	}
};

SkinDetector::Histogram::~Histogram() {};

int SkinDetector::Histogram::findCoverageIndex(double surface_to_cover, int default_value)
{
	double s = 0;
	for (int i = 0; i < histogramSize_; i++)
	{
		double bin_val = fHistogram.at<float>((userHueLt_+i)%180);
		s += bin_val;
		if (s >= surface_to_cover)
		{
			return (i+userHueLt_)%180;
		}
	}
	return default_value;
};

void SkinDetector::Histogram::findCurveThresholds(int &x1, int &x2, double percent)
{
	double sum = 0;

	for (int i = 0; i < histogramSize_; i++)
	{
		double bin_val = fHistogram.at<float>((userHueLt_+i)%180);
		sum += bin_val;
	}

	x1 = findCoverageIndex(sum * percent, -1);
	x2 = findCoverageIndex(sum * (1-percent), -1);

	if (x1 == -1)
		x1 = userHueLt_;

	if (x2 == -1)
		x2 = userHueLt_;
};

void SkinDetector::Histogram::mergeWith(SkinDetector::Histogram &source, double weight)
{
	float myweight = (float)(1-weight);
	double max_val1 = 0, max_val2 = 0;
	float f1, f2, ff1, ff2;

	minMaxLoc(source.fHistogram, NULL, &max_val2, NULL, NULL);

	if (max_val2 > 0 )
	{
		minMaxLoc(fHistogram, NULL, &max_val1, NULL, NULL);
		if (max_val1 <= 0)
		{
			for (int i = 0; i < histogramSize_; i++)
			{
				f2 = source.fHistogram.at<float>((userHueLt_+i)%180);
				fHistogram.at<float>((userHueLt_+i)%180) = f2;
			}
		}
		else
		{
			for (int i = 0; i < histogramSize_; i++)
			{
				f1 = fHistogram.at<float>((userHueLt_+i)%180);
				f2 = source.fHistogram.at<float>((userHueLt_+i)%180);

				ff1 = (f1/max_val1)*myweight;
				if (ff1 < 0)
					ff1 = -ff1;

				ff2 = (float)((f2/max_val2)*weight);
				if (ff2 < 0)
					ff2 = -ff2;

				fHistogram.at<float>((userHueLt_+i)%180) = (ff1 + ff2);
			}
		}
	}
};

void SkinDetector::setAdapt(const bool adapt) {
	adapt_ = adapt;
}

int SkinDetector::getUserHueLt() const {
	return 2*userHueLt_;
}

int SkinDetector::getUserHueUp() const {
	return 2*userHueUt_;
}

int SkinDetector::getUserValueLt() const {
	return userValueLt_;
}

int SkinDetector::getUserValueUt() const {
	return userValueUt_;
}
